#ifndef _ZT_MOTOR_H_
#define _ZT_MOTOR_H_

#include <Arduino.h>
#include <Wire.h>

class ZT_Motor 
{
    public:
        ZT_Motor();
        
		void run(int port,int speed);
		void extend_run(int MD_speed,int MC_speed);
};

ZT_Motor::ZT_Motor() 
{
	
}

void ZT_Motor::run(int port,int speed) {

  uint8_t _DIR_pin,_PWM_pin;

  switch(port)
	{

    case 1:
        _DIR_pin = 7;

        _PWM_pin = 5;

		break;

    case 2:
        _DIR_pin = 8;

        _PWM_pin = 6;

		break;
	}

	pinMode(_DIR_pin, OUTPUT);
	
	pinMode(_PWM_pin, OUTPUT);

	//speed = speed > 100 ? 100 : speed;
	
	//speed = speed < -100 ? -100 : speed;
	
	if (speed == 0)
	{
	
		digitalWrite(_DIR_pin, LOW);
	
		analogWrite(_PWM_pin, 0);
	
	}
	else if(speed > 0)
	{
		if (speed > 255) speed = 255;

		digitalWrite(_DIR_pin, LOW);
	
		//analogWrite(_PWM_pin, map(speed, 0, 100, 0, 255));
		analogWrite(_PWM_pin, speed);
	
	}
	else
	{
		if (speed < -255) speed = -255;

		digitalWrite(_DIR_pin, HIGH);
	
		//analogWrite(_PWM_pin,  map(-1*speed, 0, 100, 0, 255));
		analogWrite(_PWM_pin, -1*speed);
	
	}
}

void ZT_Motor::extend_run(int MD_speed,int MC_speed) {
	uint8_t MD_flag ,MC_flag;
	if(MD_speed >= 0 && MD_speed<= 255){MD_flag = 1;} else MD_flag = 2;
	if(MC_speed >= 0 && MC_speed<= 255){MC_flag = 1;} else MC_flag = 2;
	Wire.beginTransmission(0x28);
	Wire.write(MD_flag);
	Wire.write(MD_speed);
	Wire.write(MC_flag);
	Wire.write(MC_speed);
	Wire.endTransmission(); 
}
#endif 